package frc.circuitrunners.IO;

import com.sun.cldc.jna.BlockingFunction;
import com.sun.cldc.jna.NativeLibrary;
import com.sun.cldc.jna.Pointer;
import com.sun.cldc.jna.TaskExecutor;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Dashboard;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
//import edu.wpi.first.wpilibj.SmartDashboard;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.NIVision;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;
import frc.circuitrunners.R;

/**
 * @author James Liu
 * This class handles all Input and Output for the robot
 */
public class IO {
    
    /**
     * Boolean check for Initialized or not
     */
    public static boolean started = false;
    
    /**
     * Preset input numbers
     */
    private static final int DINum = 1, AINum = 0;
    
    /**
     * Global Input Identifiers
     * Self-explanatory, uses bitwise conditioning in IO code
     */
    public static final int JOY_RIGHT = 1 << 31;
    public static final int JOY_LEFT = 1 << 30;
    public static final int JOY_TWIST = 1 << 29;
    public static final int DIGITAL_INPUT = 1 << 28;
    public static final int ANALOG_INPUT = 1 << 27;
    public static final int X = 1 << 10;
    public static final int Y = 1 << 11;
    public static final int Z = 1 << 12;
    public static final int BUTTON = 1 << 13;
    
    /**
     * Boolean for if the robot is aimed or not
     */
    private static boolean robotAimed = false;
    
    /**
     * Input sources
     */
    private static DigitalInput digitalInputs[];
    private static AnalogChannel analogInputs[];

    /**
     * Handlers for Camera and Dashboard
     */
    public static CameraHandler cameraHandler = new CameraHandler();
    public static DashboardHandler dashboardHandler = new DashboardHandler();
    
    /**
     * Initializes the class for use
     */
    public static void init(){
        if(started){
            return;
        }
        digitalInputs = new DigitalInput[DINum];
        analogInputs = new AnalogChannel[AINum];
        for(int i = 0; i < DINum; i++){
            digitalInputs[i] = new DigitalInput(2, i + 1);
        }
        for(int i = 0; i < AINum; i++){
            analogInputs[i] = new AnalogChannel(1, i + 1);
        }
        started = true;
    }
    
    /**
     * One of the main methods of this class. Obtains the necessary input requrested.
     * Format uses the IO constants. For example, to get the right joystick's Y axis use:
     * IO.getInput(IO.JOY_RIGHT & IO.Y); Or if you want one of the buttons on the joystick use:
     * IO.getInput(IO.JOY_RIGHT & IO.BUTTON & btn); where btn is the button number you want.
     * Normally double inputs will return as normal. Boolean inputs will be returned as 1.0 or 0.0
     * where 1.0 is true and 0.0 is false.
     * @param identifierMask the input desired
     * @return the desired input value
     */
    public static double getInput(int identifierMask){
        if(!started){
            init();
        }
        if((identifierMask & JOY_RIGHT) != 0){
            return joystickInput(1, identifierMask - JOY_RIGHT);
        } else if((identifierMask & JOY_LEFT) != 0){
            return joystickInput(2, identifierMask - JOY_LEFT);
        } else if((identifierMask & JOY_TWIST) != 0){
            return joystickInput(3, identifierMask - JOY_TWIST);
        } else if((identifierMask & DIGITAL_INPUT) != 0){
            return (digitalInputs[identifierMask - DIGITAL_INPUT - 1].get()) ? 1.0 : 0.0;
        } else if((identifierMask & ANALOG_INPUT) != 0){
            return analogInputs[identifierMask - ANALOG_INPUT - 1].getVoltage(); //Need to test, unknown what values it returns
        } else {
            throw new IllegalArgumentException("GG you epic fail for calling for a nonexistant input!!");
        }
    }
    
    /**
     * Helper method for getInput, do not call outside of IO class
     * @param joy - which joystick it is getting input from
     * @param identifierMask - the identifier mask used in getInput
     * @return the value seeked
     */
    private static double joystickInput(int joy, int identifierMask){
        if(!started){
            init();
        }
        if((identifierMask & X) != 0){
            return DriverStation.getInstance().getStickAxis(joy, 1);
        } else if((identifierMask & Y) != 0){
            return DriverStation.getInstance().getStickAxis(joy, 2);
        } else if((identifierMask & Z) != 0){
            return DriverStation.getInstance().getStickAxis(joy, 3);
        } else if((identifierMask & BUTTON) != 0){
            return (((0x1 << (identifierMask - BUTTON - 1)) & DriverStation.getInstance().getStickButtons(joy)) != 0) ? 1.0 : 0.0;
         } else {
            throw new IllegalArgumentException("GG you epic fail for calling for a nonexistant input!!");
        }
    }
    
    /**
     * Instant access method, redirecting the data to the DashboardHandler
     * Logs information to the driverstation dashboard
     * @param log the information to send
     * @param name the name of the log sent
     */
    public static void log(String log, String name){
        dashboardHandler.log(log, name);
    }
    
    /**
     * Gets the PID output, or shooter rotation movement motor speed
     * @return the PID output
     */
    public static double getPIDoutput(){
        return cameraHandler.getPIDOutput();
    }
    
    /**
     * Gets the estimated distance to the goal from the Camera handler
     * @return the estimated distance to goal
     */
    public static double getEstimatedDistance(){
        return cameraHandler.getEstimatedDistance();
    }
    
    /**
     * Handles ALL the image processing on the robot, primarily used for the Shooter
     */
    public static class CameraHandler implements Runnable {
        
        /**
         * Constants used by targetToLookFor for selective purposes
         */
        final int TOP = 0, LEFT = 1, RIGHT = 2, BOTTOM = 3;
        
        /**
         * Used to sort out which target to aim for. Must be assigned to TOP, LEFT, RIGHT, or BOTTOM, nothing else will be accepted
         */
        private int targetToLookFor = TOP;
        
        /**
         * the estimated distance between the robot and the goals
         */
        private int estimatedDistance;
        
        /**
         * the speed that the rotating shooter should be spinning at
         */
        private double rotationSpeed;
        /**
         * Task Thread for native library calls
         */
        private static final TaskExecutor taskExecutor= new TaskExecutor("Vision Task");
        
        /**
         * Makes sure nothing is askew with the NIVision functions
         * @param code the NI Return code
         * @throws NIVisionException 
         */
        public static void assertCleanStatus (int code) throws NIVisionException {
            if (code == 0) {
                throw new NIVisionException(NIVision.getLastError());
            }
        }
        
        /**
         * Used to close up open holes, such as the targets
         * @param source the starting image
         * @param connectivity8 connecting at the 8 pixels aroudn or only the four cardinal ones
         * @throws NIVisionException 
         */
        private void convexHull(Pointer source, int connectivity8)  throws NIVisionException{
            assertCleanStatus(imaqConvexHullFn.call3(source.address().toUWord().toPrimitive(), source.address().toUWord().toPrimitive(), connectivity8));
        }
        
        /**
         * Used to access the native function used in convexHull
         */
        private static final BlockingFunction imaqConvexHullFn = NativeLibrary.getDefaultInstance().getBlockingFunction("imaqConvexHull");
        static { imaqConvexHullFn.setTaskExecutor(taskExecutor); }
        
        /**
         * Automatically starts a thread to handle the image processing
         */
        public CameraHandler(){
            new Thread(this).run();
        }
        
        /**
         * Executed once when the thread starts
         */
        public synchronized void run() {
            AxisCamera camera = AxisCamera.getInstance();
            int target;
            double error = 0.0, oldError = 0.0, integral = 0.0, derivative = 0.0;
            final double targetX = 0.0, kP = 0.5, kI = 0.00001, kD = 0;
            while(true){
                while(R.AUTO_AIM){
                    try{
                        ColorImage image = camera.getImage(); //Aquire Image for processing 
                        BinaryImage thresholdImage = image.thresholdRGB(143, 255, 202, 255, 140, 255); //Apply Color Threshold
                        convexHull(thresholdImage.image,1); //Close Targets with Convex Hull
                        removeSmallObjects(thresholdImage.image, 8); //Remove extraneous objects
                        ParticleAnalysisReport[] reports = thresholdImage.getOrderedParticleAnalysisReports(); //Get Particle analysis report
                        image.free(); //free memory of base image
                        thresholdImage.free(); //free memory of threoshold image
                        target = sort(reports); //Find wanted target
                        error = targetX - reports[target].center_mass_x_normalized; //Calculate error
                        derivative = oldError - error; //Calculate Derivative
                        integral += error; //calculate itnegral
                        oldError = error; //replaces oldError with the current error for derivative calculation
                        rotationSpeed = kP * error + kI * integral + kD * derivative; //pull it all toghether for the PID control
                    }catch(AxisCameraException ace){
                        ace.printStackTrace();
                    }catch(NIVisionException NIVe){
                        NIVe.printStackTrace();
                    }
                }
            }
        }
        
        /**
         * Limits the output to make sure that 
         * @param input
         * @return 
         */
        private double limit(double input){
            if(input >= 1.0){
                return 1.0;
            } else if(input <= -1.0){
                return -1.0;
            } else {
                return input;
            }
        }
        
        /**
         * Chooses which particle to aim toward
         * @param reports the particle analysis reports
         * @return the particle that is chosen to aim at
         */
        private int sort(ParticleAnalysisReport[] reports){
            int index = 0;
            switch(targetToLookFor){
                case TOP:
                    int highestY = 0;
                    for(int i = 0; i < reports.length; i++){
                        if(reports[i].center_mass_y_normalized > highestY){
                            index = i;
                        }
                    }
                    break;
                case LEFT:
                    int lowestX = 0;
                    for(int i = 0; i < reports.length; i++){
                        if(reports[i].center_mass_x_normalized< lowestX){
                            index = i;
                        }
                    }
                    break;
                case RIGHT:
                    int highestX = 0;
                    for(int i = 0; i < reports.length; i++){
                        if(reports[i].center_mass_x_normalized > highestX){
                            index = i;
                        }
                    }
                    break;
                case BOTTOM:
                    int lowestY = 0;
                    for(int i = 0; i < reports.length; i++){
                        if(reports[i].center_mass_y_normalized < lowestY){
                            index = i;
                        }
                    }
                    break;
                default:
                    break;
            }
            return index;
        }
        
        //TODO: Implement removeSmallObjects Method
        private BinaryImage removeSmallObjects(Pointer image, int iterations){
            throw new IllegalArgumentException("Function Not supported");
        }
        
        /**
         * Returns the current calculated distance from he goal
         * @return distance to the goal
         */
        public synchronized int getEstimatedDistance(){
            return estimatedDistance;
        }
        
        /**
         * Returns the speed at which the shooter should be rotating at
         * @return 
         */
        public synchronized double getPIDOutput(){
            return rotationSpeed;
        }
    }
    
    /**
     * Handles the dashboard processing, sends and recieves data from the dashboard
     */
    public static class DashboardHandler implements Runnable {
        
        /**
         * Default Constructor
         */
        public DashboardHandler(){
            //SmartDashboard.init();
            new Thread(this).run();
        }
        
        /**
         * Logs information to the dashboard
         * @param log information to be logged
         * @param name name of the log made
         */
        public void log(String log, String name){
            //SmartDashboard.log(log, name);
        }
        
        /**
         * 
         */
        public void run() {
            while(true){
                Dashboard lowDashData = DriverStation.getInstance().getDashboardPackerLow();
                lowDashData.addCluster();{
                    lowDashData.addBoolean(robotAimed);
                    lowDashData.addDouble(cameraHandler.getEstimatedDistance());
                }
                lowDashData.finalizeCluster();
                lowDashData.commit();
            }
        }
    }
}
